import math
import random
import sys
from math import *

import numpy as np
import pygame
from pygame import *
from scipy.interpolate import splev, splprep,splrep


class Node(object):
    def __init__(self, point, parent):
        super(Node, self).__init__()
        self.point = point
        self.parent = parent


XDIM = 280 * 2
YDIM = 280 * 2
windowSize = [XDIM, YDIM]  #窗口尺寸
delta = 10.0
GAME_LEVEL = 3
GOAL_RADIUS = 10  #目标半径
MIN_DISTANCE_TO_ADD = 1.0
NUMNODES = 9000
FPS = 1000
pygame.init()
fpsClock = pygame.time.Clock()
screen = pygame.display.set_mode(windowSize)
white = 255, 255, 255
black = 25, 25, 25
red = 255, 0, 0
blue = 0, 128, 0
green = 0, 0, 255
cyan = 0, 180, 105

count = 0
rectObs = []
dets = np.loadtxt('dets.txt', delimiter=',')
print(dets)


def fitSpline(points):  #构造光滑曲线
    pts = np.array(points)
    tck, u = splprep(pts.T, u=None, s=0.0, per=1)
    u_new = np.linspace(u.min(), u.max(), 1000)
    x_new, y_new = splev(u_new, tck, der=0)
    return list(zip(x_new, y_new))


def dist(p1, p2):  #两点之间的距离
    return sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) *
                (p1[1] - p2[1]))


def point_circle_collision(p1, p2, radius):
    distance = dist(p1, p2)
    if (distance <= radius):
        return True
    return False


def step_from_to(p1, p2):
    if dist(p1, p2) < delta:
        return p2
    else:
        theta = atan2(p2[1] - p1[1], p2[0] - p1[0])
        return p1[0] + delta * cos(theta), p1[1] + delta * sin(theta)


def collides(p):  #check if point collides with the obstacle
    for rect in rectObs:
        if rect.collidepoint(p) == True:
            return True
    return False


def get_random_clear():
    while True:
        p = random.random() * XDIM, random.random() * YDIM
        noCollision = collides(p)
        if noCollision == False:
            return p


def init_obstacles(configNum):  #初始化界面，界面内的障碍物
    global rectObs
    rectObs = []
    print("config " + str(configNum))
    if (configNum == 0):
        rectObs.append(
            pygame.Rect((XDIM / 2.0 - 50, YDIM / 2.0 - 100), (100, 200)))
    if (configNum == 1):
        rectObs.append(pygame.Rect((100, 50), (200, 150)))
        rectObs.append(pygame.Rect((400, 200), (200, 100)))
    if (configNum == 2):
        rectObs.append(pygame.Rect((100, 50), (200, 150)))
    if (configNum == 3):
        for i in list(range(280)):
            for j in list(range(280)):
                if dets[i, j] > 1840 or dets[i, j] < 1700:
                    rectObs.append(pygame.Rect((i * 2, j * 2), (2, 2)))

    for rect in rectObs:
        pygame.draw.rect(screen, black, rect)


def reset():
    global count
    screen.fill(white)
    init_obstacles(GAME_LEVEL)
    count = 0


def main():
    global count

    initPoseSet = False
    initialPoint = Node(None, None)
    goalPoseSet = False
    goalPoint = Node(None, None)
    currentState = 'init'

    nodes = []
    MyNodes = []
    x = []
    reset()

    while True:
        if currentState == 'init':
            pygame.display.set_caption(
                'Select Starting Point and then Goal Point')
            fpsClock.tick(10)
        elif currentState == 'goalFound':
            currNode = goalNode.parent
            pygame.display.set_caption('Goal Reached')

            MyNodes.append(goalPoint.point)
            MyNodes.append(currNode.point)
            while currNode.parent != None:
                pygame.draw.line(screen, red, currNode.point,
                                 currNode.parent.point)
                currNode = currNode.parent
                MyNodes.append(currNode.point)

            optimizePhase = True

            currentState = 'Smooth'
        elif currentState == 'optimize':
            fpsClock.tick(0.5)
            pass
        elif currentState == 'Smooth':
            pygame.display.set_caption('Smoothing')
            #print(MyNodes)
            print("######")
            pts = np.array(MyNodes)
            x_=pts.T[0].T
            y_=pts.T[1].T
            #print(x_)
            #x_=np.arange(x_)
            #y_=np.arange(y_)
            #splprep()
            tck, u = splprep(pts.T, u=None, s=0.0) # Here pts.T means Transpose of the pts.
            #print(u.min())
            #print(u.max())
            print(tck)
            #print(pts.T)
            u_new = np.linspace(u.min(), u.max(), 100)
            x_new, y_new = splev(u_new, tck, der=0)
            print(x_new)
            print(y_new)
            #tck= splrep(x_, y_ ,s=0.0)  # Here pts.T means Transpose of the pts.
            #print(u.min())
            #print(u.max())
            #print(tck)
            #print("XXXXXXX")
            #x_new = np.arange(min(x_), max(x_), 0.001)
            #print(x_new)
            #y_new = splev(x_new, tck, der=0)
            #u_new = np.linspace(u.min(), u.max(), 1000)
            #x_new, y_new = splev(u_new, tck, der=0)

            x = zip(x_new, y_new)
            x = list(x)
            #y=np.array(x)
            #print(y)
            #print(x[0])
            currentState = 'Smooth Over!'
            np.savetxt("line.txt", x,fmt='%f',delimiter=',')
        elif currentState == 'Smooth Over!':
            pygame.display.set_caption('Smooth Over!')
            tempNum = len(x)
            #print(tempNum)
            for i in range(tempNum-1):
                pygame.draw.line(screen, (0,0,0), x[i], x[i + 1],5)
            

        elif currentState == 'buildTree':
            count = count + 1
            pygame.display.set_caption('Performing RRT')
            if count < NUMNODES:
                foundNext = False
                while foundNext == False:
                    rand = get_random_clear()
                    parentNode = nodes[0]
                    for p in nodes:
                        if dist(p.point, rand) <= dist(parentNode.point, rand):
                            newPoint = step_from_to(p.point, rand)
                            if collides(newPoint) == False:
                                parentNode = p
                                foundNext = True

                newnode = step_from_to(parentNode.point, rand)
                nodes.append(Node(newnode, parentNode))
                pygame.draw.line(screen, cyan, parentNode.point, newnode)

                if point_circle_collision(newnode, goalPoint.point,
                                          GOAL_RADIUS):
                    currentState = 'goalFound'

                    goalNode = nodes[len(nodes) - 1]

            else:
                print("Ran out of nodes... :(")
                return

        for e in pygame.event.get():
            if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE):
                sys.exit("Exiting")
            if e.type == MOUSEBUTTONDOWN:
                print('mouse down')
                if currentState == 'init':
                    if initPoseSet == False:
                        nodes = []
                        if collides(e.pos) == False:
                            print('initiale point set: ' + str(e.pos))

                            initialPoint = Node(e.pos, None)
                            nodes.append(initialPoint)  # Start in the center
                            initPoseSet = True
                            pygame.draw.circle(screen, red, initialPoint.point,
                                               GOAL_RADIUS)
                    elif goalPoseSet == False:
                        print('goal point set: ' + str(e.pos))
                        if collides(e.pos) == False:
                            goalPoint = Node(e.pos, None)
                            goalPoseSet = True
                            pygame.draw.circle(screen, green, goalPoint.point,
                                               GOAL_RADIUS)
                            currentState = 'buildTree'
                else:
                    currentState = 'init'
                    initPoseSet = False
                    goalPoseSet = False
                    reset()

        pygame.display.update()
        fpsClock.tick(FPS)


if __name__ == '__main__':
    main()
